%reduce 7 control point sequence into 5 if it satisfies the curvature and
%the safety constraint
function [finalSequence, theLength, theCurvature, theSafety]=optimizeCurve(totalSequence, passingIdx, ...
    theCurvature, theSafety, sites, triangles, triangleGrid)

finalSequence=totalSequence(:,1:(passingIdx(1)-1));

for t=1:(length(passingIdx)-1)
    finalSequence=[finalSequence, totalSequence(:,passingIdx(t):(passingIdx(t)+1))];
    oldCurve=totalSequence(:,(passingIdx(t)+2):(passingIdx(t+1)-2));
    %newCurve=oldCurve;
    newCurve=reduceCurve(oldCurve, theCurvature, theSafety, sites, triangles, triangleGrid);
    finalSequence=[finalSequence, newCurve, totalSequence(:,(passingIdx(t+1)-1))];
end
finalSequence=[finalSequence, totalSequence(:,end)];

%calculate criteria
numCurves=(size(finalSequence,2)-1)/2;
theLength=zeros(1,numCurves);
for c=1:numCurves
    %current curve of interst
    P=finalSequence(:,(2*c-1));
    Q=finalSequence(:,(2*c));
    R=finalSequence(:,(2*c+1));

    theLength(c)=findLength(P,Q,R);
    theCurvature(c)=findCurvature(P,Q,R);
    theSafety(c)=findSafety(P,Q,R,sites,triangles, triangleGrid);
end

numCurves=(size(finalSequence,2)-1)/2;
theLength=zeros(1,numCurves);
for c=1:numCurves
    %current curve of interst
    P=finalSequence(:,(2*c-1));
    Q=finalSequence(:,(2*c));
    R=finalSequence(:,(2*c+1));

    theLength(c)=findLength(P,Q,R);
    theCurvature(c)=findCurvature(P,Q,R);
    theSafety(c)=findSafety(P,Q,R,sites,triangles, triangleGrid);
end

theCurvature=max(theCurvature);
theSafety=min(theSafety);

end

function curve=reduceCurve(curve, theCurvature, theSafety, sites, triangles, triangleGrid)

i=1;
while i+6<=size(curve,2)
    newCurve=[curve(:,i:(i+1)), (curve(:,i+1)+curve(:,i+5))./2, curve(:,(i+5):(i+6))];
    curvature=findCurvature(newCurve(:,1), newCurve(:,2), newCurve(:,3));
    if curvature>theCurvature
        i=i+2;
        continue;
    end
    curvature=findCurvature(newCurve(:,3), newCurve(:,4), newCurve(:,5));
    if curvature>theCurvature
        i=i+2;
        continue;
    end
    safety=findSafety(newCurve(:,1), newCurve(:,2),newCurve(:,3),sites, triangles, triangleGrid);
    if safety<theSafety
        i=i+2;
        continue;
    end
    safety=findSafety(newCurve(:,3), newCurve(:,4), newCurve(:,5),sites,triangles,triangleGrid);
    if safety<theSafety
        i=i+2;
        continue;
    end
    curve=[curve(:,1:(i-1)), newCurve, curve(:,(i+7):end)];
end

end